Support for raycast

[eduardo98m]

Support for raycast

[jcarpent]

What are you exact needs @eduardo98m? Which concrete applications do you foresee?
@eduardo98m We would be happy to help you to contribute to hpp-fcl by implementing this feature. Do you agree?

From what I have read in other forums a quick fix might be to create a very long and thin cylinder, or use a small sphere in continuous collision detection mode.

These solutions won't be efficient. You should rather look at faster solutions ;)

[eduardo98m]

Hello @jcarpent

I basically need to get the point of contact between a ray (defined with a start and an endpoint) and any other collider.

The application i want to get from theses is being able to simulate a Lidar sensor (or ultrasonic, infrared, etc. exterioceptive sensor)

Here is an rough sketch of the functionality:

Here is also my basic implementation using a capsule collider (I figured it would be faster than a cylinder):

(The code below is part of my physics engine project, thats why I perform the collision test with multiple objects).

std::vector<vec3> World::raycast(vec3 start, vec3 end)
{

    // Create a capsule in hpp::fcl
    scalar lenght = ti::magnitude(end - start);
    scalar radius = 0.001; // if we take 1 m as the unit, this should be 1 [mm]

    std::shared_ptr<hpp::fcl::Capsule> ray_collider = std::make_shared<hpp::fcl::Capsule>(radius, lenght);

    vec3 position = 0.5*(start + end);

    vec3 direction = ti::normalize(end - start);
    vec3 up = {0.0, 1.0, 0.0};

    // Create a quaternion representing the capsule orientation
    quat orientation = ti::quat_from_axis_angle(direction, 0.0f);

    // Rotate the quaternion by 90 degrees around the up vector to align the capsule with the raycast
    orientation = orientation * ti::quat_from_axis_angle(up, M_PI / 2.0f);


    // Now we need to get the orientation of the capsule from the two points
    hpp::fcl::Transform3f transform = ti::get_eigen_transform(position,
                                                              orientation);

    std::vector<vec3> points;
    // Now we need to check the capsule collider with the rest of the bodies of the world
    for (int i = 0; i < this->bodies.size(); i++)
    {
        hpp::fcl::CollisionResult col_res;
        hpp::fcl::CollisionRequest col_req;

        hpp::fcl::collide(ray_collider.get(),
                          transform,
                          this->bodies[i].collider_info.get(),
                          ti::get_eigen_transform(this->bodies[i].position,
                                                  this->bodies[i].orientation),
                          col_req,
                          col_res);

        if (col_res.isCollision())
        {
            vec3 normal = {0.0, 0.0, 0.0};
            vec3 contrac_point_1 = {0.0, 0.0, 0.0};
            vec3 contrac_point_2 = {0.0, 0.0, 0.0};
            hpp::fcl::Contact contact = col_res.getContact(0);
            points.push_back(ti::from_eigen(contact.pos));
        }
    }

    return points;
}

I am not really sure how to implement the feature directly in hpp::fcl, but im guessing that creating a ray collider and figuring out its signed distance function might be the way to go.

[jcarpent]

Thanks @eduardo98m for sharing your thoughts on the topic.

An efficient solution should rely on the support function, derived for each primitive and which is at the core of the GJK and EPA algorithms for checking the collision between convex objects.

[lmontaut]

Hi @eduardo98m,
If you represent your rays by capsules with zero radius, hppfcl will treat your capsule as a segment. You can then call collide and things should work out of the box. Let us know if this solution works for you :)

[eduardo98m]

I tried using the capsule with zero radius (and also tried the cylinder), and im getting some strange results. For some reason I only get one contact per collision, and the points I get while colliding with a sphere collider are not really the ones that are expected. I made a video so it could be better visualized:
https://youtu.be/EnQ7Yq9Gm5E

Here is the code of the raycast (Im not sure if there is a parameter in the collision request i should change to get better results):

std::vector<vec3> World::raycast(vec3 start, vec3 end)
{

    // Create a capsule in hpp::fcl
    scalar lenght = ti::magnitude(end - start);
    scalar radius = 0; //

    std::shared_ptr<hpp::fcl::Cylinder> ray_collider = std::make_shared<hpp::fcl::Cylinder>(radius, lenght);

    vec3 position = 0.5 * (start + end);
    vec3 direction = ti::normalize(end - start);
    vec3 up = {0.0, 0.0, 1.0};
    vec3 cross = ti::cross(up, direction);
    vec3 axis = cross/ti::magnitude(cross);
    scalar angle  = ti::acos(ti::dot(direction, up));
    quat orientation = ti::quat_from_axis_angle(axis, angle) ;
    
    hpp::fcl::CollisionObject* ray = new hpp::fcl::CollisionObject(ray_collider);
    ray->setTransform(ti::get_eigen_transform(position, orientation));

    ray->computeAABB();

    std::vector<vec3> points;
    // Now we need to check the capsule collider with the rest of the bodies of the world
    for (int i = 0; i < this->bodies.size(); i++)
    {

        hpp::fcl::CollisionObject* col_obj = new hpp::fcl::CollisionObject(this->bodies[i].collider_info);
        col_obj->setTransform(
                ti::get_eigen_transform(this->bodies[i].position,
                                                  this->bodies[i].orientation)
        );
        col_obj->computeAABB();

        if (ray->getAABB().contain(col_obj->getAABB())){
            hpp::fcl::CollisionResult col_res;
            hpp::fcl::CollisionRequest col_req;

            col_req.num_max_contacts = 16;

            hpp::fcl::collide(ray,
                            col_obj,
                            col_req,
                            col_res);

            if (col_res.isCollision())
            {
                // vec3 point;
                // scalar min_dist = INFINITY;
                // std::cerr << "N contacs : " << col_res.numContacts() << "\n"; 
                // for (int i = 0; i< col_res.numContacts(); i++ ){
                //     vec3 candidate = ti::from_eigen(col_res.getContact(i).pos);
                //     // scalar dist = ti::magnitude(start - candidate);
                //     // if (dist < min_dist){
                //     //     min_dist = dist;
                //     //     point = candidate;
                //     // }

                //     points.push_back(candidate);
                // }
                points.push_back(ti::from_eigen(col_res.nearest_points[1]));
            }
        }
        
    }

    return points;
}

[lmontaut]

Hey @eduardo98m, thanks a lot for the video! That's really helpful. I read your issue too quickly and didn't realize you also needed the projection of the ray onto the object. I thought you only to know yes/no collision.

The contact points you are getting are coherent with collision detection in the sense that it corresponds to the middle point of the separating vector (i.e the vector that brings the two shapes in touching collision).
Anyway, it's indeed not the point you want in your application.

Let me think more about your problem and see if there is an easy way to solve it with hpp-fcl and get back to you!

[lmontaut]

Hi @eduardo98m, one way to solve your problem might be to do two successive calls to collide and distance:

• First is a collide call between your geometry and the ray represented by a capsule with zero radius. This tells you whether or not the ray is colliding with the geometry.
• If it is the case, then you can call distance between the geometry and the origin of the ray. The origin of the ray can be represented as a sphere with zero radius. You will get 2 witness points, one should be the origin of the ray and the other one will be the orthogonal projection of the origin of the ray onto the geometry.

Let us know if this works for you!

[eduardo98m]

Hello @lmontaut sorry for the late response, I tried what your suggestion but im not getting the desired result, because i don't get the points the capsule is passing trough the geometry (P2 in the figure), but the nearest point of the colliding shape to the origin of the ray (P1).

Maybe i am getting something wrong, or there is an operation that im missing, but so far thats what I have been able to get.