#include "dart/dynamics/CapsuleShape.hpp"
#include <cmath>
#include "dart/dynamics/CylinderShape.hpp"
#include "dart/dynamics/SphereShape.hpp"
#include "dart/math/Helpers.hpp"