Calculate and visualize correct contact points
Some work on sat tests