Adding tested geometry discretization capability.

Continued from #340. New pull request to avoid merge conflict and git history problems.