Mainly added data stuctures.
Finish geometry, add some algorithms.