Proper physics code.
Merged with tuomo's stuff