Add initial code. Ported from WillowGarage sources