Update SoftFCLMeshCollisionDetector.cpp