Started collision detection

Started working on collision detection. Added geomerty and
transformations classes, and started adding obstacle and collision
avoidance steering behaviours