add a computation of the minimal distance
use of PQP for the collision check
start to implement dynamic simulation using pqp
remove std::cout
include collision with meshes -- stil to work on normals
return the position of the collision in the world frame