forcefit displacement matrix implementation in C