Add new safety geometry types prisms and capsules

[gywn]

Introduce two new geometry types that embody the mathematical models used for calculating minimum distances in the realm of safety-functions-related collision checking:

1. GT_Prism: Right prisms with arbitrary non-convex cross-section and finite height, which represent “walls” or “safe zones”. In the FCL plugin, the prism is modeled with an fcl::Container that consists of several inward-facing fcl::Halfspace. It uses _meshcollision to store its information, _meshcollision.vertices[2 * i].xy describes the arbitrary nonconvex cross section of the right prism. A copy of the height information is also stored in _vGeomData.y.

2. GT_Capsule: Capsules, representing “robot model” or “gripper model”. It shares a similar data structure with cylinders.

The existing geometry types GT_Sphere and GT_Box can also be used as safety geometries.

[gywn]

@kanbouchou @snozawa

[rdiankov]

thanks~