It looks like you are re-inventing the Bounce2 library. Have a look at how it is done within that code.