Broad phase with subsequent narrow phase

[JonasBreuling]

Maybe this seems like a dump question but it might be due to my misunderstanding of the library. I try to perform a broad phase collision detection and if some overlapping AABB's are found, a subsequent narrow phase should identify the true contacts (if there are any).

#include <iostream>
#include <memory>
#include <coal/broadphase/broadphase_collision_manager.h>
#include <coal/broadphase/broadphase_dynamic_AABB_tree.h>
#include <coal/broadphase/default_broadphase_callbacks.h>
#include <coal/collision.h>
#include <coal/math/transform.h>

int main()
{
    // create two shapes
    std::shared_ptr<coal::Sphere> shape1 = std::make_shared<coal::Sphere>(0.5);
    std::shared_ptr<coal::Box> shape2 = std::make_shared<coal::Box>(3, 2, 1);
    std::shared_ptr<coal::Box> shape3 = std::make_shared<coal::Box>(0.1, 1, 3);

    // random placements in 3D space
    coal::Transform3s T1;
    T1.setQuatRotation(coal::Quaternion3f::UnitRandom());
    T1.setTranslation(coal::Vec3s::Random());
    coal::Transform3s T2 = coal::Transform3s::Identity();
    T2.setQuatRotation(coal::Quaternion3f::UnitRandom());
    T2.setTranslation(coal::Vec3s::Random());
    coal::Transform3s T3 = coal::Transform3s::Identity();
    T3.setQuatRotation(coal::Quaternion3f::UnitRandom());
    T3.setTranslation(coal::Vec3s::Random());

    /**********************************
     * broad phase collision detection
     **********************************/
    // create and populate collision objects
    std::vector<coal::CollisionObject*> collision_objects;
    collision_objects.push_back(new coal::CollisionObject(shape1, T1));
    collision_objects.back()->collisionGeometry()->computeLocalAABB();
    collision_objects.push_back(new coal::CollisionObject(shape2, T2));
    collision_objects.back()->collisionGeometry()->computeLocalAABB();
    collision_objects.push_back(new coal::CollisionObject(shape3, T3));
    collision_objects.back()->collisionGeometry()->computeLocalAABB();

    // ceate, populate and setup collision manager
    coal::DynamicAABBTreeCollisionManager broad_phase_manager;
    for (const auto& object : collision_objects)
        broad_phase_manager.registerObject(object);
    broad_phase_manager.setup();

    // collision check with broadphase
    coal::CollisionCallBackDefault collision_callback;
    broad_phase_manager.collide(&collision_callback);

    std::cout << "Collision with broadphase: "
              << (collision_callback.data.result.isCollision() ? "true" : "false") << std::endl;

    const std::vector<coal::Contact> contacts = collision_callback.data.result.getContacts();

    /***********************************
     * narrow phase collision detection
     ***********************************/
    coal::CollisionRequest col_req;
    coal::CollisionResult col_res;

    int n_contacts = contacts.size();
    std::cout << "number of contacts in broad phase: " << n_contacts << std::endl;
    for (const auto& contact : contacts) {
        // contact position
        auto pos = contact.pos;
        std::cout << "contact position: " << pos.transpose() << std::endl;

        // contact normal
        auto normal = contact.normal;
        std::cout << "contact normal: " << normal.transpose() << std::endl;

        /*******************************************************************
         * Question: How to extract the CollisionObject or Transform3s from 
         * contact.o1 and contact.o2 to perform narrow search?
         *******************************************************************/
        // auto o1 = contact.o1;
        // auto o2 = contact.o1;
        // coal::collide(contact.o1->getCollisionObject(), contact.o2->getCollisionObject(), col_req, col_res);

        std::cout << "Collision? " << (col_res.isCollision() ? "true" : "false") << "\n";
        if (col_res.isCollision()) {
            // do something here...
        }
        
        // prepare for next collision request
        col_res.clear();
    }

    return 0;
}

Everything seems clear to me up to the line const std::vector<coal::Contact> contacts = collision_callback.data.result.getContacts();, where I get a std::vector of Contact's. These contacts contain both involved CollisionGeometry's but not information of the involved CollisionObject's that are required by a subsequent call to coal::collide. Since this seems to be a very basic task, I'm confident that this is a user error on my side.

[Everest-Zhao]

I believe the approach you have taken might be redundant. Based on my understanding, the broad phase manager automatically invokes the narrow phase's collide function. Therefore, the contacts you receive are not limited to collisions between AABBs but also include the actual shapes, such as spheres or boxes, that you provided. As a result, there is no need to perform the narrow phase collision again.

However, if you intend to explicitly use the narrow phase collision, you must provide the original CollisionObject or both the CollisionGeometry and its associated transform to the narrow phase.

[JonasBreuling]

@Everest-Zhao: That sounds very reasonable. So in the end I only have to populate the collision manager, which does all the job for me. Maybe this can be verified by someone of the code owners.