Add caching of M0 to solveLCP
Remove unused second deriv. in contactConstraints
Fix idx error in contactPositionsFromCollisionData
Comment out debug printing in constructModelmex