Ну в расчетах поможет аналитическая геометрия. Алгоритмы можешь посмотреть
тут.
Насчет как все организовать - просто следуй парадигме ООП. Как в большинстве игр, например написанных на c++ все объекты - экземпляры своих классов. Например есть класс отрисовщика сцены, есть класс мира, содержащего игровые объекты и производящего вычисления, есть классы для самих игровых объектов которые ловят события из этого мира и генерируют свои.