Add testDrop2
Add testEquationOfMotion.cpp
Add debug code to SoftFCLMeshCollisionDetector.cpp
Try to fix soft contact issue