Это удивительно сложная проблема, если вы мало знаете о физике или математике.
Вам нужно знать о законах движения Ньютона. Это пара дифференциальных уравнений, поэтому вам нужно знать, как их решить.
Проблема становится проще, если вы предполагаете, что определенные вещи не важны для интересующего вас поведения: жесткие и деформируемые сферы, трение и другие факторы.
Ответ во многом зависит от того, что вы хотели бы сделать.
Быстрый взгляд на размещенный вами код показывает, что лучшее, на что вы можете надеяться, это то, что у вас правильная, но не оптимальная реализация. Вероятно, вы не в состоянии понять физику и математику достаточно хорошо, чтобы решить ее самостоятельно.
Я бы запустил Google для физического движка на выбранном вами языке и использовал бы идеализацию, которую уже закодировал кто-то другой.