Energy-based 6-DOF penetration depth computation for penalty-based haptic rendering algorithms