Added unit tests and Get Link Geometry feature

Added some unit tests based on google test framework.

Added GetLinkShapes function that returns a map from link id -> shape,
where shape is a vector of 3-d points.