Remove cost from src/CMakeLists.txt.
Fix constness and a bug in interpolate3d.
Fix some issues and add some tests.