Started work on normal detection

Cluster in direction space is next, also: speed