What convention for normal direction?

[alaurenzi]

Dear developers,
is there any documentation available about the convention that is used for computing the normal (as available from DistanceResult, e.g.)?

Is it always pointing from body 2 to body 1 as in the capsule-capsule computation (at least from latest master)?

Edit: I am asking this since I was syncing our fork with you latest tag v2.4.5, and I believe that now the box-capsule distance computes a normal whose direction is different if the two shapes are in collision or not. This is a preliminary result based on the failure of some of our unit tests, and I'd need a bit more time to confirm it and provide a minimum reproducible example if needed.

Thanks,
Arturo

[lmontaut]

Hi @alaurenzi,
The documentation is in the C++ code and python bindings of the devel branch.

In short, in the newest hpp-fcl version, the normal always point from shape 1 to shape 2, whether or not there is penetration and regardless of the pair of shapes.
This change is due to a uniformization of hpp-fcl's behavior that we had to do. Previously, depending on which algo was used internally, and thus depending on the pair of shapes, there were multiple conventions for the normal.
The recent fixes of hpp-fcl have made it so the normal is always defined pointing from shape 1 to shape 2.

Note: the tag 2.4.5 is the last tag before moving to coal / hpp-fcl version 3 (see #612 #596 ).

[alaurenzi]

Hi @lmontaut, if I understand correctly, the normal must always point from the witness point on body 1 to the witness point on body 2. This means that, while shifting one of the two bodies along the normal direction, the normal will change sign when collision happens, right?

I tried to verify this on a pair of capsules. Please have a look to the following simple program.

#include <hpp/fcl/distance.h>
#include <hpp/fcl/shape/geometric_shapes.h>
#include <iostream>

void compute_dist(hpp::fcl::CollisionObject * o1,
                  hpp::fcl::CollisionObject * o2)
{
    hpp::fcl::DistanceRequest dreq;
    dreq.enable_nearest_points = true;

    hpp::fcl::DistanceResult dres;
    dres.clear();

    hpp::fcl::distance(o1, o2, dreq, dres);
    auto w12 = dres.nearest_points[0] - dres.nearest_points[1];

    std::cout << "w1       = " << dres.nearest_points[0].transpose() << "\n";
    std::cout << "w2       = " << dres.nearest_points[1].transpose() << "\n";
    std::cout << "w12_norm = " << w12.normalized().transpose() << "\n";
    std::cout << "normal   = " << dres.normal.transpose() << "\n";
    std::cout << "distance = " << dres.min_distance << "\n\n";
}

int main()
{
    // unused
    auto box_geom = std::make_shared<hpp::fcl::Box>(
        1, 1, 1
        );

    auto capsule_geom = std::make_shared<hpp::fcl::Capsule>(
        1, 1
        );

    auto o1 = std::make_shared<hpp::fcl::CollisionObject>(capsule_geom);

    auto o2 = std::make_shared<hpp::fcl::CollisionObject>(capsule_geom);

    o2->setTranslation({1, 0, 0});
    compute_dist(o1.get(), o2.get());

    o2->setTranslation({3, 0, 0});
    compute_dist(o1.get(), o2.get());
}

It basically checks a capsule-capsule distance twice (collision and no collision). On my machine the output is

w1       =    1    0 -0.5
w2       =    0    0 -0.5
w12_norm = 1 0 0
normal   = 1 0 0
distance = -1

w1       =    1    0 -0.5
w2       =    2    0 -0.5
w12_norm = -1  0  0
normal   = 1 0 0
distance = 1

You can see that the normal does not flip sign (whereas the normalized difference between witness points does). Can you confirm that this is not the expected behavior?

[lmontaut]

Hi @alaurenzi,
No, the normal always points from body 1 to body 2 like so:

In other words, if pi is a witness point on shape i, the normal is:

• p2 - p1 normalized if there is no penetration
• p1 - p2 normalized if there is a penetration

The reason behind this convention is that if we denote n the normal and d is the separation distance (which is signed, >0 if no penetration, <= 0 if penetration), then v = n * d is the separation vector.
The separation vector is the vector such that if body 1 is translated by v, the shapes are brought in touching contact (their separation distance is exactly 0).

Note that there may be multiple vectors that bring the shapes to a touching contact but the separation vector is the vector with the smallest norm that does that.

[alaurenzi]

@lmontaut all clear now, thank you very much