Add ~= approximate equality matcher
Use Vec3D and Point3D matchers in tests