Issues with GImpact contact test

[ghost]

Hi,

I have found an issue with the GImpact contact test.
It reports false contacts if the triangles lay behind each other (and do not touch each other) but are close enough to be within their aabbs.
This can be the case if you have "small" objects or meshes where both sides of an triangle are visible and relevant for the collision.
GImpact mesh should work double sided.

I investigated the problem further and build an example consisting of only two objects which have only one triangle each (see end of this post), that shows the issue.
I looked in the code and found the issues. I fix it so that only triangles get reported as contact that really intersect each other and no longer all triangles that lay behind each other and within the aabb of a triangle.

I am happily providing a pull request for this, if there is any chance to get it merged. Is there any thing I need to pay special attention to?

#include "btBulletDynamicsCommon.h"
#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"

#include <iostream>
#include <vector>
#include <memory>

struct TriangleMeshData
{
    std::vector<btScalar> vertices;
    std::vector<int> indices;
};

TriangleMeshData createTriangleMesh1()
{
    return TriangleMeshData{
        {
             0, 0, 0,
             1, 0, 1,
             1, 1, 1,
        },
        {0, 1, 2}
    };
}

TriangleMeshData createTriangleMesh2()
{
    return TriangleMeshData{
        {
            0, 1,   1,
            1, 1,   0,
            1, 0.1, 0,
        },
        {0, 1, 2}
    };
}

struct BulletObject
{
    explicit BulletObject(TriangleMeshData meshData)
        : m_meshData{std::move(meshData)}
    {
        m_triangleIndexVertexArray = std::make_unique<btTriangleIndexVertexArray>(
            m_meshData.indices.size() / 3, m_meshData.indices.data(),
            3 * sizeof(meshData.indices[0]), static_cast<int>(m_meshData.vertices.size()),
            m_meshData.vertices.data(), 3 * sizeof(m_meshData.vertices[0]));

        m_gImpactMeshShape = std::make_unique<btGImpactMeshShape>(m_triangleIndexVertexArray.get());
        m_gImpactMeshShape->setLocalScaling(btVector3(1.0f, 1.0f, 1.0f));
        m_gImpactMeshShape->setMargin(MARGIN);
        m_gImpactMeshShape->updateBound();

        m_compountShape = std::make_unique<btCompoundShape>();
        btTransform transform;
        transform.setIdentity();
        m_compountShape->addChildShape(transform, m_gImpactMeshShape.get());
        m_compountShape->setMargin(MARGIN);
        
        m_collisionObject = std::make_unique<btCollisionObject>();
        m_collisionObject->setCollisionShape(m_compountShape.get());
    }

    btCollisionObject* getBtCollisionObject()
    {
        return m_collisionObject.get();
    }

private:
    static constexpr btScalar MARGIN = 1.0e-4;

    TriangleMeshData m_meshData;
    std::unique_ptr<btTriangleIndexVertexArray> m_triangleIndexVertexArray;
    std::unique_ptr<btGImpactMeshShape>         m_gImpactMeshShape;
    std::unique_ptr<btCompoundShape>            m_compountShape;
    std::unique_ptr<btCollisionObject>          m_collisionObject;
};

struct BulletContactResultCallback : public btCollisionWorld::ContactResultCallback
{
public:

    bool needsCollision(btBroadphaseProxy* proxy0) const override
    {
        return true;
    }

    btScalar addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap,
                             int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap,
                             int partId1, int index1) override
    {
        if (colObj0Wrap->getCollisionObject() == colObj1Wrap->getCollisionObject())
            return 0;
        btVector3 ptA = cp.getPositionWorldOnA();
        btVector3 ptB = cp.getPositionWorldOnB();
        double distance = cp.getDistance();

        std::cout << "Contact : (" << ptA.getX() << "," << ptA.getY() << "," << ptA.getZ()
                  << ") (" << ptB.getX() << "," << ptB.getY() << "," << ptB.getZ()
                  << ") & d: " << distance << '\n';
        return 0;
    }
};

int main()
{
    btDefaultCollisionConfiguration collisionConfiguration;
    btCollisionDispatcher dispatcher(
        static_cast<btCollisionConfiguration *>(&collisionConfiguration));

    btScalar sceneSize = btScalar(100);
    btVector3 worldAabbMin(-sceneSize, -sceneSize, -sceneSize);
    btVector3 worldAabbMax(sceneSize, sceneSize, sceneSize);

    btAxisSweep3 broadphase(worldAabbMin, worldAabbMax, 16000);

    btCollisionWorld collisionWorld(
        static_cast<btDispatcher *>(&dispatcher), static_cast<btBroadphaseInterface *>(&broadphase),
        static_cast<btCollisionConfiguration *>(&collisionConfiguration));

    // Register Gimpact algorithm
    btGImpactCollisionAlgorithm::registerAlgorithm(&dispatcher);

    auto triangle1 = BulletObject(createTriangleMesh1());
    auto triangle2 = BulletObject(createTriangleMesh2());
    collisionWorld.addCollisionObject(triangle1.getBtCollisionObject());
    collisionWorld.addCollisionObject(triangle2.getBtCollisionObject());

    BulletContactResultCallback callback;

    collisionWorld.contactTest(triangle1.getBtCollisionObject(), callback);

    collisionWorld.removeCollisionObject(triangle1.getBtCollisionObject());
    collisionWorld.removeCollisionObject(triangle2.getBtCollisionObject());
    return 0;
}