Terrain contact points improvements mk2

Rebase of #130 against current master.