improving cpp version of bodyKin